#ifndef OBSTACLELASER
#define OBSTACLELASER
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <cmath>
#include <iostream>
#include <string>
#include <group6/DetectedObject.h>
#include <group6/DetectedObjectArray.h>
#define Pii 3.1415926536



class ObstacleLaser
{
public:
    ObstacleLaser(ros::NodeHandle& nh, std::string publish_frame);
    ~ObstacleLaser(){}

    void generateLaserScan(const sensor_msgs::LaserScan::ConstPtr& real_scan);

    void updateObstacle(const group6::DetectedObject::ConstPtr& objects);
    void updateObstacles(const group6::DetectedObjectArray::ConstPtr& objects);
    void addObstacle(double x, double y, double r);
    void addWallObstacle(double x, double y, double r);

    void addWallObstacles();

    double limitAngle(double prevAngle);
    double mes_index_to_angle(int mes_num);
    int angle_to_index(double angle);
    double angle_difference(double angle1, double angle2);


private:
    // ROS handles
    ros::NodeHandle mNode;
    ros::Publisher scan_pub;
    //Laser publish frame
    std::string pub_frame;
    //Scan message.
    sensor_msgs::LaserScan scan;
    ros::Time scan_time;


    std::vector< std::vector<double> > obstaclePoints;
    std::vector< std::vector<double> > wallObstaclePoints;
    std::vector<double> newObstacle;
    int num_readings;
    std::vector<double> ranges;
    std::vector<double> intensities;
    tf::TransformListener mListener;
    tf::StampedTransform currentTransform;

    //Parameters to contain laser pose data.
    double LaserX;
    double LaserY;
    double LaserAngle;

    //Obstacle range info.
    double OMA; //Obstacle direction from the robot, map coordinates
    double OLA; //Obstacle direction from the robot, robot coordinates
    double OLAdiff; //Obstacle field of view
    double OLD; //Obstacle distance

    int OLIstart;
    int OLIend;

    double alpha;
    double rsin;
    double gamma;
    double theta;
    double point_distance;
};


#endif
